/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2010, Jack Pien
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#include <termios.h>
#include <fcntl.h>
#include <unistd.h> 

#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>

#include "daido_driver.h"
#include <sys/time.h>

class DaidoDriver 
{
private:
    // Serial port
    int fd_;

    boost::mutex stop_motor_mutex_;
    ros::Time stop_time_;
    bool need_stop_motor_;
    ros::NodeHandle n_;

    ros::Timer motor_halt_timer_;
    ros::Subscriber sub_;

    // Current twist geometry state
    geometry_msgs::Twist cur_base_controller_state_;

    uint32_t getUs() {
        // struct timeval tv;
        // suseconds_t tus;
        // time_t tsec;
        // uint32_t totalTime;

        // gettimeofday( &tv, NULL );
        // tus = tv.tv_usec;
        // tsec = tv.tv_sec;
        // tsec *= 1000000;
        // tsec += tus;
        // totalTime = (uint32_t)(tsec);
        ros::Time the_time = ros::Time::now();
        uint32_t total_time = (uint32_t)(the_time.toSec()*1000000.0);
        return total_time;
    }

    bool drive( int16_t rwv, int16_t lwv ) {
        uint32_t clk = getUs();
        uint32_t nbytes;
        DDmsgDirectDrive msg;
        uint8_t buf[4096]; 

/*         float rf, lf; */
/*         short rwv, lwv; */
/*         rf = (float)((rand() % 101)) / 100.0f; */
/*         lf = (float)((rand() % 101)) / 100.0f; */
/*         rwv = (1000 * rf) - 500; */
/*         lwv = (1000 * lf) - 500; */
        msg.rwvelocity = rwv;
        msg.lwvelocity = lwv;
        ROS_INFO( "DD drive: %d, %d", rwv, lwv ); 
        nbytes = msgPackDirectDrive( &msg, DD_PKT_MSG_OFFSET(buf) );
        nbytes = ddPackHeader( clk, nbytes, buf );
        
        if( write( fd_, buf, nbytes ) != nbytes ) {
            ROS_INFO( "Error writing to serial" );
            return false;
        }
        ROS_INFO( "Controller: clk %u, %d bytes to drive", clk, 
                  nbytes );

        // Get the ack
        uint32_t sender_ts;
        uint32_t msg_ts;
        if( ddGetAck( fd_, &sender_ts, &msg_ts ) == 0 ) {
            ROS_INFO( "Ack not received properly" );
            return false;
        }

        return true;
    }

    void handle_fail_transport() {
        tcflush(fd_, TCIOFLUSH);
    }

    void baseControllerCallback(const geometry_msgs::Twist::ConstPtr& msg) {
        ros::Time the_time = ros::Time::now();
        ROS_INFO("%llu", (long long unsigned int)(the_time.toNSec()));
        ROS_INFO("%f", the_time.toSec());
        ROS_INFO("I heard: linear[%f, %f, %f], angular[%f, %f, %f]", 
                 msg->linear.x, msg->linear.y, msg->linear.z,
                 msg->angular.x, msg->angular.y, msg->angular.z);

        int16_t rwv = 0;
        int16_t lwv = 0;

        // Yaw
        if( msg->angular.z != 0 ) {
            // Right is negative z
            rwv = (int16_t)(100.0 * msg->angular.z * 1.0);
            lwv = (int16_t)(100.0 * msg->angular.z * -1.0);
        } else if( msg->linear.x != 0 ) {
            rwv = (int16_t)(400.0 * msg->linear.x);
            lwv = (int16_t)(400.0 * msg->linear.x);
        }             

        // Try to send drive command
        bool success = drive( rwv, lwv );
        
        // Get lock to modify stop motor conditions
        stop_motor_mutex_.lock();

        // Send driver command
        if( success == false ) {
            // Stop motors ASAP
            handle_fail_transport();
            stop_time_ = ros::Time::now();
        } else {
            // Stop motors 2 seconds later
            stop_time_ = ros::Time::now() + ros::Duration(1.0);
        }

        // Only need to stop motors if we haven't stopped it already
        // or if there was an error
        need_stop_motor_ = true;
        // if( write( fd_, "12345678901234567890123456789012345678901234567890|0\n", 52 ) == 52 ) {
        //     char buf[4096]; 
        //     //usleep(200000);
        //     //tcflush(fd_, TCIOFLUSH);
        //     int read_bytes = read( fd_, buf, sizeof(buf) );
        //     if( read_bytes > 0 ) {
        //         buf[read_bytes] = NULL;
        //         ROS_INFO("Heard: %s", buf);
        //     } else {
        //         ROS_INFO("Nothing came back");
        //     }
        // } else {
        //     ROS_INFO("Error writing out to serial port");
        // }

        stop_motor_mutex_.unlock();
    }

    /**
     * Callback to check if we need to halt the motors
     */
    void motorHaltCallback(const ros::TimerEvent& e) {
        // Get a lock on the need stop motor check
        stop_motor_mutex_.lock();

        if( need_stop_motor_ && e.current_real >= stop_time_ ) {
            ROS_INFO("Halting motors");
            bool success = drive( 0, 0 );
            if( success == false ) {
                // OK try again
                ROS_INFO("Halting motors failed - trying again after flushing serial port buffers");
                handle_fail_transport();
                success = drive( 0, 0 );
                if( success == false ) {
                    ROS_INFO("Halting motors failed after 2nd retry");
                    handle_fail_transport();
                }
            }
            need_stop_motor_ = false;
        }

        stop_motor_mutex_.unlock();
    }

public:
    /**
     * Constructor
     */
    DaidoDriver() {
        stop_time_ = ros::Time::now();
        need_stop_motor_ = false;

        // Create a timer event to stop the motor
        motor_halt_timer_ = 
            n_.createTimer(ros::Duration(0.1), 
                           &DaidoDriver::motorHaltCallback, this);

        // Subscribe to message
        sub_ = n_.subscribe("/base_controller/command", 5, 
                            &DaidoDriver::baseControllerCallback, this);

        // Init base controller state
        cur_base_controller_state_.linear.x = 0;
        cur_base_controller_state_.linear.y = 0;
        cur_base_controller_state_.linear.z = 0;
        cur_base_controller_state_.angular.x = 0;
        cur_base_controller_state_.angular.y = 0;
        cur_base_controller_state_.angular.z = 0;

        // Get some params from the param server
        std::string serial_port("/dev/ttyUSB0");
        if( n_.getParam("serial_port", serial_port) ) {
        }
        ROS_INFO("Communicating to platform via: %s", serial_port.c_str());

        // Set up serial port to write out to Daido platform
        fd_ = open(serial_port.c_str(), O_RDWR | O_NOCTTY );

        if( fd_ >= 0 ) {
            struct termios pts;
            
            tcgetattr(fd_, &pts);
            cfsetispeed(&pts, B115200);   // set output baud to 115200
            cfsetospeed(&pts, B115200);   // set output baud to 115200
            pts.c_cc[VMIN] = 0;
            pts.c_cc[VTIME] = 50;
            pts.c_iflag &= ~(IGNPAR);
            pts.c_iflag |= (IGNBRK);
            pts.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
            pts.c_oflag &= ~OPOST;
            // fcntl(fd, F_SETFL, FNDELAY);
            
            tcsetattr(fd_, TCSANOW, &pts);
        } else {
            ROS_INFO("Could not open specified serial port %s",
                     serial_port.c_str());
        }
    }

    /**
     * Loop
     */
    void loop() {
        /**
         * ros::spin() will enter a loop, pumping callbacks. 
         * With this version, all
         * callbacks will be called from within this thread 
         * (the main one).  ros::spin()
         * will exit when Ctrl-C is pressed, or the node is 
         * shutdown by the master.
         */
        ros::spin();
    }

};

int main(int argc, char **argv)
{
    /**
     * The ros::init() function needs to see argc and argv so that it 
     * can perform
     * any ROS arguments and name remapping that were provided at the 
     * command line. For programmatic
     * remappings you can use a different version of init() which 
     * takes remappings
     * directly, but for most command-line programs, passing argc and argv 
     * is the easiest
     * way to do it.  The third argument to init() is the name of the node.
     *
     * You must call one of the versions of ros::init() before using any other
     * part of the ROS system.
     */
    ros::init(argc, argv, "daido_driver");

    DaidoDriver ddrv;

    ddrv.loop();

    return 0;
}
